#include "MMC5983MA.h"
#include <MPython.h>

void MMC5983MA::begin()
{
    Wire.begin();
    mPython.begin();
    judgeId();

    if(chip == 1) // MMC5983MA
    {
        writeReg(MAG_ADDR, 0x0A, 0b00000000);
        writeReg(MAG_ADDR, 0x0B, 0b10110000);
        writeReg(MAG_ADDR, 0x09, 0b00100001);
    }else if(chip == 2){ //MMC5603NJ
        writeReg(MAG_ADDR, 0x1C, 0x80);
        delay(100);
        writeReg(MAG_ADDR, 0x1A, 255);
        writeReg(MAG_ADDR, 0x1B, 0b10100001);
        writeReg(MAG_ADDR, 0x1C, 0b00000000);
        writeReg(MAG_ADDR, 0x1D, 0b10010000);
        delay(100);
    }
}

void MMC5983MA::judgeId()
{
    //判断product_ID
    uint8_t retry, id;
    for (retry = 0; retry < 5; retry++)
    {
        id = readReg(MAG_ADDR, 0x39);
        if(id == 16){
            chip = 2;
            productId = 16;
        }else{
            chip = 1;
            productId = 48;
        }
        // Serial.println(chip);
    }
}

void MMC5983MA::cal()
{
    display.fillScreen(0);
    display.setCursorLine(1);
    display.printLine("一、 水平放置掌控版");
    display.setCursorLine(2);
    display.printLine("      以垂直地面为轴");
    display.setCursorLine(3);
    display.printLine("      旋转几周");
    readMagRaw(rx, ry, rz);
    max_x = rx;
    min_x = max_x;
    max_y = ry;
    min_y = max_y;
    max_z = rz;
    min_z = max_z;
    unsigned long ticks_start = millis();
    while((millis()-ticks_start)<15000){
        readMagRaw(rx, ry, rz);
        min_x = min(rx, min_x);
        min_y = min(ry, min_y);
        max_x = max(rx, max_x);
        max_y = max(ry, max_y);
        delay(100);
    }
    xoffs = (min_x + max_x)/2;
    yoffs = (min_y + max_y)/2;
    display.fillScreen(0);
    display.setCursorLine(1);
    display.printLine("二、 竖直放置掌控版");
    display.setCursorLine(2);
    display.printLine("      以垂直地面为轴");
    display.setCursorLine(3);
    display.printLine("      旋转几周");
    readMagRaw(rx, ry, rz);
    ticks_start = millis();
    while((millis()-ticks_start)<15000){
        readMagRaw(rx, ry, rz);
        min_z = min(rz, min_z);
        max_z = max(rz, max_z);
        delay(100);
    }
    zoffs = (min_z + max_z)/2;
    display.fillScreen(0);
    display.setCursorLine(1);
    display.printLine("校准完成");
    delay(3000);
    display.fillScreen(0);
}

void MMC5983MA::peeling()
{
    readMagRaw(rx, ry, rz);
    peeling_x = rx;
    peeling_y = ry;
    peeling_z = rz;
    is_peeling = 1;
}

void MMC5983MA::clear_peeling()
{
    peeling_x = 0.0;
    peeling_y = 0.0;
    peeling_z = 0.0;
    is_peeling = 0;
}


float MMC5983MA::get_Xraw()
{
    if(chip == 1){
        readMagRaw(rx, ry, rz);
        return rx* 0.25;
    }else if(chip == 2){
        readMagRaw(rx, ry, rz);
        if(xoffs){
            return -0.0625 * (rx - xoffs);
        }else{
            return -0.0625 * (rx - 524288);
        }
    }
    
}

float MMC5983MA::get_Yraw()
{
    if(chip == 1){
        readMagRaw(rx, ry, rz);
        return ry* 0.25;
    }else if(chip == 2){
        readMagRaw(rx, ry, rz);
        if(yoffs){
            return -0.0625 * (ry - yoffs);
        }else{
            return -0.0625 * (ry - 524288);
        }
    }
}

float MMC5983MA::get_Zraw()
{
    if(chip == 1){
        readMagRaw(rx, ry, rz);
        return rz* 0.25;
    }else if(chip == 2){
        readMagRaw(rx, ry, rz);
        if(zoffs){
            return 0.0625 * (rz - zoffs);
        }else{
            return 0.0625 * (rz - 524288);
        }
    }
}

float MMC5983MA::get_heading()
{
    float temp_x,temp_y,temp_z;
    float heading;
    if(chip == 1){
        readMagRaw(rx, ry, rz);
        temp_x = rx - xoffs;
        temp_y = ry - yoffs;
        temp_z = rz - zoffs;
        float heading = atan2(temp_y,-temp_x)*(180/3.14159265)+180+3;
        return heading;
    }else if(chip == 2){
        if(xoffs){
            readMagRaw(rx, ry, rz);
            temp_x = -(rx - xoffs);
            temp_y = -(ry - yoffs);
            heading = atan2(temp_y, -temp_x) * (180 / 3.14159265) + 180 + 3;
        }else{
            heading = atan2(get_Yraw(), -get_Xraw()) * (180 / 3.14159265) + 180 + 3;
        }
        return heading;
}
    
}

float MMC5983MA::get_field_strength()
{
    if(chip == 1){    
        readMagRaw(rx, ry, rz);
        if(is_peeling == 1){
            return sqrt((rx - peeling_x)*(rx - peeling_x) + (ry - peeling_y)*(ry - peeling_y) + (rz - peeling_z)*(rz - peeling_z))*0.25;
        }
        return sqrt(rx * rx + ry *ry + rz *rz )*0.25;
    }else if (chip == 2)
    {
        readMagRaw(rx, ry, rz);
        if(is_peeling == 1)
            return (sqrt(pow(rx - peeling_x, 2) + pow(ry - peeling_y, 2) + pow(rz - peeling_z , 2)))*0.0625;
        return (sqrt(pow(get_Xraw(), 2) + pow(get_Yraw(), 2) + pow(get_Zraw(), 2)));
    }
    
}

void MMC5983MA::readMagRaw(int32_t &x, int32_t &y, int32_t &z){

    if(chip == 1){
        writeReg(MAG_ADDR, 0x09, 0b00001000);   //perform set pulse
        delay(5);
        writeReg(MAG_ADDR, 0x09, 0b00010000);   //perform reset pulse
        delay(5);
        writeReg(MAG_ADDR, 0x09, 0b00000001);   //request new ADC measurement
        delay(10);

        Wire.beginTransmission(MAG_ADDR);
        Wire.write(0x00);                       //start at X axis MSB
        Wire.endTransmission();
        Wire.requestFrom(MAG_ADDR, 6);          //read until Z axis LSB
        x = ((Wire.read() << 8) | Wire.read()) - 32768;   //16-bit unsigned format, shifted
        y = ((Wire.read() << 8) | Wire.read()) - 32768;   //16-bit unsigned format, shifted
        z = ((Wire.read() << 8) | Wire.read()) + 32768;   //Z axis seems to be inverted, not sure why
    }else if(chip == 2){
        uint8_t status, buf[9]={0};
        writeReg(MAG_ADDR, 0x1B, 0b00001000);
        delay(5);
        writeReg(MAG_ADDR, 0x1B, 0b00000001);
        delay(5);

        while (1)
        {
            delay(25);
            status = readReg(MAG_ADDR, 0x18);
            if (status & 0x40)
                break;
        }
        readReg(MAG_ADDR, 0x00, buf, 9);
        x = (buf[0] << 12) | (buf[1] << 4) | (buf[6] >> 4);
        y = (buf[2] << 12) | (buf[3] << 4) | (buf[7] >> 4);
        z = (buf[4] << 12) | (buf[5] << 4) | (buf[8] >> 4);
    }

  
}

uint8_t MMC5983MA::readReg(uint8_t addr, uint8_t reg){

  Wire.beginTransmission(addr);
  Wire.write(reg);
  Wire.endTransmission();
  Wire.requestFrom(addr, 1);
  return Wire.read();
}

void MMC5983MA::readReg(uint8_t addr, uint8_t reg, uint8_t *buf, uint8_t size){

    Wire.beginTransmission(addr);
    Wire.write(reg);
    Wire.endTransmission();
    Wire.requestFrom(addr, size);
    for(uint8_t i=0; i<size; i++){
        buf[i] = Wire.read();
    }
}

void MMC5983MA::writeReg(uint8_t addr, uint8_t reg, uint8_t val){

  Wire.beginTransmission(addr);
  Wire.write(reg);
  Wire.write(val);
  Wire.endTransmission();
}
